/*
 * Everything needed for the sensors to work properly
 */
#include "sensor.h"

int i=0;

//*** Initialisiere alle Infrarot- und Ultraschall-Sensoren ***
void init_sensors() {
	put_uart("[INIT] Sensors");
	put_uart(CRLF);
	init_IR_Sensors();
	init_i2c_bus();
	return;
}

/**
 * Collects all the data from Infrared and Ultrasonic sensors, by first starting
 * the Ultrasonic devices, the reading the Infrared data and finally
 * collecting the triggered Ultrasonic feedback
 */
void collect_Data() {
	//start_all_US_Measure();
	start_US_Measure(US_IDs[0]);
	start_US_Measure(US_IDs[1]);
	start_US_Measure(US_IDs[2]);
	_delay_ms(100);
	US_vals[0] = read_US_Data(US_IDs[0]);
	US_vals[1] = read_US_Data(US_IDs[1]);
	US_vals[2] = read_US_Data(US_IDs[2]);

	start_US_Measure(US_IDs[3]);
	start_US_Measure(US_IDs[4]);
	_delay_ms(100);
	read_all_IR_Data();
	US_vals[3] = read_US_Data(US_IDs[3]);
	US_vals[4] = read_US_Data(US_IDs[4]);
	//TODO Delays checken
	//read_all_US_Data();
	return;
}

void print_all_Data(void) {
	put_uart("[INFO] IR Data");
	put_uart(CRLF);
	for (i=0; i<ARRAY_SIZE(IR_IDs); i++) {
		put_uart(i+1);
		put_uart(": ");
		put_uarti(IR_vals[i]);
		put_uart("; ");
	}
	//	put_uart(CRLF);

	//	put_uart("[INFO] US Data");
	//	put_uart(CRLF);
	for (i=0; i<ARRAY_SIZE(US_IDs); i++) {
		put_uarti(i+1);
		put_uart(": ");
		put_uarti(US_vals[i]);
		put_uart("; ");
	}
	put_uart(CRLF);
	return;
}
